//
// File: FLAE.cpp
//
// MATLAB Coder version            : 5.1
// C/C++ source code generated on  : 22-Feb-2021 03:11:30
//

// Include Files
#include "FLAE.h"
#include "FLAE_data.h"
#include "FLAE_rtwutil.h"
#include "det.h"
#include "eig.h"
#include "rt_nonfinite.h"
#include "sqrt.h"
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <cstring>

// Function Declarations
static double rt_atan2d_snf(double u0, double u1);
static double rt_powd_snf(double u0, double u1);

// Function Definitions
//
// Arguments    : double u0
//                double u1
// Return Type  : double
//
static double rt_atan2d_snf(double u0, double u1)
{
  double y;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = rtNaN;
  } else if (rtIsInf(u0) && rtIsInf(u1)) {
    int b_u0;
    int b_u1;
    if (u0 > 0.0) {
      b_u0 = 1;
    } else {
      b_u0 = -1;
    }

    if (u1 > 0.0) {
      b_u1 = 1;
    } else {
      b_u1 = -1;
    }

    y = std::atan2(static_cast<double>(b_u0), static_cast<double>(b_u1));
  } else if (u1 == 0.0) {
    if (u0 > 0.0) {
      y = RT_PI / 2.0;
    } else if (u0 < 0.0) {
      y = -(RT_PI / 2.0);
    } else {
      y = 0.0;
    }
  } else {
    y = std::atan2(u0, u1);
  }

  return y;
}

//
// Arguments    : double u0
//                double u1
// Return Type  : double
//
static double rt_powd_snf(double u0, double u1)
{
  double y;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = rtNaN;
  } else {
    double d;
    double d1;
    d = std::abs(u0);
    d1 = std::abs(u1);
    if (rtIsInf(u1)) {
      if (d == 1.0) {
        y = 1.0;
      } else if (d > 1.0) {
        if (u1 > 0.0) {
          y = rtInf;
        } else {
          y = 0.0;
        }
      } else if (u1 > 0.0) {
        y = 0.0;
      } else {
        y = rtInf;
      }
    } else if (d1 == 0.0) {
      y = 1.0;
    } else if (d1 == 1.0) {
      if (u1 > 0.0) {
        y = u0;
      } else {
        y = 1.0 / u0;
      }
    } else if (u1 == 2.0) {
      y = u0 * u0;
    } else if ((u1 == 0.5) && (u0 >= 0.0)) {
      y = std::sqrt(u0);
    } else if ((u0 < 0.0) && (u1 > std::floor(u1))) {
      y = rtNaN;
    } else {
      y = std::pow(u0, u1);
    }
  }

  return y;
}

//
// Arguments    : const double r_base[9]
//                const double b_base[9]
//                const double weights[3]
//                int method
//                creal_T Q[4]
// Return Type  : void
//
void FLAE(const double r_base[9], const double b_base[9], const double weights[3],
          int method, creal_T Q[4])
{
  static const signed char b_b[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0,
    0, 1 };

  creal_T V[16];
  creal_T unusedU0[16];
  creal_T T1;
  creal_T T2;
  creal_T dc;
  creal_T dc1;
  creal_T dc2;
  double WW[16];
  double b_MM[16];
  double c_MM[16];
  double MM[9];
  double b_tmp[9];
  double b_y[9];
  double q[4];
  double x_data[4];
  double ar;
  double d;
  double lambda;
  double t;
  double y;
  int MM_tmp;
  int i;
  int iter;

  //  Fast Linear Quaternion Attitude Estimator from Vector Observations
  //  Published in IEEE Transactions on Automation Science and Engineering
  //  Authors: Jin Wu, Zebo Zhou et al.
  //  Copytight (c) 2016
  std::memset(&MM[0], 0, 9U * sizeof(double));
  for (iter = 0; iter < 3; iter++) {
    d = weights[iter];
    ar = b_base[3 * iter];
    y = b_base[3 * iter + 1];
    t = b_base[3 * iter + 2];
    for (i = 0; i < 3; i++) {
      lambda = r_base[i + 3 * iter];
      MM[3 * i] += d * ar * lambda;
      MM_tmp = 3 * i + 1;
      MM[MM_tmp] += d * y * lambda;
      MM_tmp = 3 * i + 2;
      MM[MM_tmp] += d * t * lambda;
    }
  }

  //  H1_matrix
  //  H2_matrix
  //  H3_matrix
  WW[0] = MM[0];
  WW[4] = 0.0;
  WW[8] = -MM[2];
  WW[12] = MM[1];
  WW[1] = 0.0;
  WW[5] = MM[0];
  WW[9] = MM[1];
  WW[13] = MM[2];
  WW[2] = -MM[2];
  WW[6] = MM[1];
  WW[10] = -MM[0];
  WW[14] = 0.0;
  WW[3] = MM[1];
  WW[7] = MM[2];
  WW[11] = 0.0;
  WW[15] = -MM[0];
  b_MM[0] = MM[4];
  b_MM[4] = MM[5];
  b_MM[8] = 0.0;
  b_MM[12] = -MM[3];
  b_MM[1] = MM[5];
  b_MM[5] = -MM[4];
  b_MM[9] = MM[3];
  b_MM[13] = 0.0;
  b_MM[2] = 0.0;
  b_MM[6] = MM[3];
  b_MM[10] = MM[4];
  b_MM[14] = MM[5];
  b_MM[3] = -MM[3];
  b_MM[7] = 0.0;
  b_MM[11] = MM[5];
  b_MM[15] = -MM[4];
  c_MM[0] = MM[8];
  c_MM[4] = -MM[7];
  c_MM[8] = MM[6];
  c_MM[12] = 0.0;
  c_MM[1] = -MM[7];
  c_MM[5] = -MM[8];
  c_MM[9] = 0.0;
  c_MM[13] = MM[6];
  c_MM[2] = MM[6];
  c_MM[6] = 0.0;
  c_MM[10] = -MM[8];
  c_MM[14] = MM[7];
  c_MM[3] = 0.0;
  c_MM[7] = MM[6];
  c_MM[11] = MM[7];
  c_MM[15] = MM[8];
  for (i = 0; i < 16; i++) {
    WW[i] = (WW[i] + b_MM[i]) + c_MM[i];
  }

  switch (method) {
   case 0:
    {
      double T1_tmp;
      double a;
      double b;
      double c;
      double old_lambda;

      //  method name 'symbolic'
      c = coder::det(WW);
      for (i = 0; i < 3; i++) {
        b_tmp[3 * i] = MM[i];
        b_tmp[3 * i + 1] = MM[i + 3];
        b_tmp[3 * i + 2] = MM[i + 6];
      }

      b = -8.0 * coder::b_det(b_tmp);
      for (i = 0; i < 3; i++) {
        for (iter = 0; iter < 3; iter++) {
          b_y[i + 3 * iter] = (MM[i] * MM[iter] + MM[i + 3] * MM[iter + 3]) +
            MM[i + 6] * MM[iter + 6];
        }
      }

      a = -2.0 * ((b_y[0] + b_y[4]) + b_y[8]);
      lambda = (2.0 * rt_powd_snf(a, 3.0) + 27.0 * (b * b)) - 72.0 * a * c;
      T1_tmp = a * a + 12.0 * c;
      T1.re = -4.0 * rt_powd_snf(T1_tmp, 3.0) + lambda * lambda;
      T1.im = 0.0;
      coder::b_sqrt(&T1);
      T1.re += lambda;
      ar = T1.re;
      if ((T1.im == 0.0) && (T1.re >= 0.0)) {
        T1.re = rt_powd_snf(ar, 0.33333333333333331);
        T1.im = 0.0;
      } else {
        if (T1.im == 0.0) {
          if (T1.re < 0.0) {
            d = T1.re;
            T1.re = std::log(std::abs(d));
            T1.im = 3.1415926535897931;
          } else {
            d = T1.re;
            T1.re = std::log(d);
            T1.im = 0.0;
          }
        } else if ((std::abs(T1.re) > 8.9884656743115785E+307) || (std::abs
                    (T1.im) > 8.9884656743115785E+307)) {
          d = T1.re;
          ar = T1.im;
          y = T1.im;
          t = T1.re;
          T1.re = std::log(rt_hypotd_snf(d / 2.0, ar / 2.0)) +
            0.69314718055994529;
          T1.im = rt_atan2d_snf(y, t);
        } else {
          d = T1.re;
          ar = T1.im;
          y = T1.im;
          t = T1.re;
          T1.re = std::log(rt_hypotd_snf(d, ar));
          T1.im = rt_atan2d_snf(y, t);
        }

        T1.re *= 0.33333333333333331;
        T1.im *= 0.33333333333333331;
        if (T1.im == 0.0) {
          d = T1.re;
          T1.re = std::exp(d);
          T1.im = 0.0;
        } else if (rtIsInf(T1.im) && rtIsInf(T1.re) && (T1.re < 0.0)) {
          T1.re = 0.0;
          T1.im = 0.0;
        } else {
          t = std::exp(T1.re / 2.0);
          d = T1.im;
          ar = T1.im;
          T1.re = t * (t * std::cos(d));
          T1.im = t * (t * std::sin(ar));
        }
      }

      ar = 2.5198420997897459 * T1_tmp;
      if (T1.im == 0.0) {
        c = ar / T1.re;
        lambda = 0.0;
      } else if (T1.re == 0.0) {
        if (ar == 0.0) {
          c = 0.0 / T1.im;
          lambda = 0.0;
        } else {
          c = 0.0;
          lambda = -(ar / T1.im);
        }
      } else {
        old_lambda = std::abs(T1.re);
        lambda = std::abs(T1.im);
        if (old_lambda > lambda) {
          t = T1.im / T1.re;
          y = T1.re + t * T1.im;
          c = (ar + t * 0.0) / y;
          lambda = (0.0 - t * ar) / y;
        } else if (lambda == old_lambda) {
          if (T1.re > 0.0) {
            t = 0.5;
          } else {
            t = -0.5;
          }

          if (T1.im > 0.0) {
            y = 0.5;
          } else {
            y = -0.5;
          }

          c = (ar * t + 0.0 * y) / old_lambda;
          lambda = (0.0 * t - ar * y) / old_lambda;
        } else {
          t = T1.re / T1.im;
          y = T1.im + t * T1.re;
          c = t * ar / y;
          lambda = (t * 0.0 - ar) / y;
        }
      }

      T2.re = (-4.0 * a + c) + 1.5874010519681994 * T1.re;
      T2.im = lambda + 1.5874010519681994 * T1.im;
      coder::b_sqrt(&T2);
      ar = 29.393876913398135 * b;
      if (T2.im == 0.0) {
        c = ar / T2.re;
        y = 0.0;
      } else if (T2.re == 0.0) {
        if (ar == 0.0) {
          c = 0.0 / T2.im;
          y = 0.0;
        } else {
          c = 0.0;
          y = -(ar / T2.im);
        }
      } else {
        old_lambda = std::abs(T2.re);
        lambda = std::abs(T2.im);
        if (old_lambda > lambda) {
          t = T2.im / T2.re;
          y = T2.re + t * T2.im;
          c = (ar + t * 0.0) / y;
          y = (0.0 - t * ar) / y;
        } else if (lambda == old_lambda) {
          if (T2.re > 0.0) {
            t = 0.5;
          } else {
            t = -0.5;
          }

          if (T2.im > 0.0) {
            y = 0.5;
          } else {
            y = -0.5;
          }

          c = (ar * t + 0.0 * y) / old_lambda;
          y = (0.0 * t - ar * y) / old_lambda;
        } else {
          t = T2.re / T2.im;
          y = T2.im + t * T2.re;
          c = t * ar / y;
          y = (t * 0.0 - ar) / y;
        }
      }

      lambda = T2.re * T2.im;
      lambda += lambda;
      t = -(T2.re * T2.re - T2.im * T2.im) - 12.0 * a;
      T1_tmp = t - c;
      T1.re = T1_tmp;
      old_lambda = -lambda - y;
      T1.im = old_lambda;
      coder::b_sqrt(&T1);
      dc.re = T1_tmp;
      dc.im = old_lambda;
      coder::b_sqrt(&dc);
      d = t + c;
      dc1.re = d;
      ar = -lambda + y;
      dc1.im = ar;
      coder::b_sqrt(&dc1);
      dc2.re = d;
      dc2.im = ar;
      coder::b_sqrt(&dc2);
      q[0] = 0.20412414523193151 * (T2.re - T1.re);
      q[1] = 0.20412414523193151 * (T2.re + dc.re);
      q[2] = -0.20412414523193151 * (T2.re + dc1.re);
      q[3] = -0.20412414523193151 * (T2.re - dc2.re);
      T1.re = T1_tmp;
      T1.im = old_lambda;
      coder::b_sqrt(&T1);
      dc.re = T1_tmp;
      dc.im = old_lambda;
      coder::b_sqrt(&dc);
      dc1.re = d;
      dc1.im = ar;
      coder::b_sqrt(&dc1);
      dc2.re = d;
      dc2.im = ar;
      coder::b_sqrt(&dc2);
      x_data[0] = 0.20412414523193151 * (T2.re - T1.re);
      x_data[1] = 0.20412414523193151 * (T2.re + dc.re);
      x_data[2] = -0.20412414523193151 * (T2.re + dc1.re);
      x_data[3] = -0.20412414523193151 * (T2.re - dc2.re);
      if (!rtIsNaN(x_data[0])) {
        iter = 1;
      } else {
        boolean_T exitg1;
        iter = 0;
        MM_tmp = 2;
        exitg1 = false;
        while ((!exitg1) && (MM_tmp < 5)) {
          if (!rtIsNaN(x_data[MM_tmp - 1])) {
            iter = MM_tmp;
            exitg1 = true;
          } else {
            MM_tmp++;
          }
        }
      }

      if (iter == 0) {
        lambda = q[0];
      } else {
        lambda = q[iter - 1];
        i = iter + 1;
        for (MM_tmp = i; MM_tmp < 5; MM_tmp++) {
          d = q[MM_tmp - 1];
          if (lambda < d) {
            lambda = d;
          }
        }
      }

      for (i = 0; i < 16; i++) {
        WW[i] -= lambda * static_cast<double>(b_b[i]);
      }

      d = WW[4] / WW[0];
      ar = d;
      WW[4] = d;
      d = WW[5] - WW[1] * d;
      WW[5] = d;
      WW[6] -= WW[2] * ar;
      d = WW[8] / WW[0];
      ar = d;
      WW[8] = d;
      d = WW[9] - WW[1] * d;
      WW[9] = d;
      WW[10] -= WW[2] * ar;
      d = WW[12] / WW[0];
      ar = d;
      WW[12] = d;
      d = WW[13] - WW[1] * d;
      WW[14] -= WW[2] * ar;
      ar = WW[9] / WW[5];
      y = ar;
      WW[9] = ar;
      ar = WW[8] - WW[4] * ar;
      WW[8] = ar;
      WW[10] -= WW[6] * y;
      ar = d / WW[5];
      y = ar;
      WW[13] = ar;
      ar = WW[12] - WW[4] * ar;
      WW[14] -= WW[6] * y;
      d = WW[14] / WW[10];
      y = d;
      WW[14] = d;
      d = ar - WW[8] * d;
      WW[13] -= WW[9] * y;

      //  return the result Q
      lambda = 3.3121686421112381E-170;
      old_lambda = std::abs(d);
      if (old_lambda > 3.3121686421112381E-170) {
        y = 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / 3.3121686421112381E-170;
        y = t * t;
      }

      old_lambda = std::abs(WW[13]);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(WW[14]);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      if (1.0 > lambda) {
        y = y * lambda * lambda + 1.0;
        lambda = 1.0;
      } else {
        t = 1.0 / lambda;
        y += t * t;
      }

      y = lambda * std::sqrt(y);
      Q[0].re = d / y;
      Q[0].im = 0.0;
      Q[1].re = WW[13] / y;
      Q[1].im = 0.0;
      Q[2].re = WW[14] / y;
      Q[2].im = 0.0;
      Q[3].re = -1.0 / y;
      Q[3].im = 0.0;
    }
    break;

   case 1:
    {
      double old_lambda;

      //  method name 'eig'
      coder::eig(WW, V, unusedU0);

      //  return the result Q
      lambda = 3.3121686421112381E-170;
      old_lambda = std::abs(V[12].re);
      if (old_lambda > 3.3121686421112381E-170) {
        y = 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / 3.3121686421112381E-170;
        y = t * t;
      }

      old_lambda = std::abs(V[12].im);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[13].re);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[13].im);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[14].re);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[14].im);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[15].re);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(V[15].im);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      y = lambda * std::sqrt(y);
      if (V[12].im == 0.0) {
        Q[0].re = V[12].re / y;
        Q[0].im = 0.0;
      } else if (V[12].re == 0.0) {
        Q[0].re = 0.0;
        Q[0].im = V[12].im / y;
      } else {
        Q[0].re = V[12].re / y;
        Q[0].im = V[12].im / y;
      }

      if (V[13].im == 0.0) {
        Q[1].re = V[13].re / y;
        Q[1].im = 0.0;
      } else if (V[13].re == 0.0) {
        Q[1].re = 0.0;
        Q[1].im = V[13].im / y;
      } else {
        Q[1].re = V[13].re / y;
        Q[1].im = V[13].im / y;
      }

      if (V[14].im == 0.0) {
        Q[2].re = V[14].re / y;
        Q[2].im = 0.0;
      } else if (V[14].re == 0.0) {
        Q[2].re = 0.0;
        Q[2].im = V[14].im / y;
      } else {
        Q[2].re = V[14].re / y;
        Q[2].im = V[14].im / y;
      }

      if (V[15].im == 0.0) {
        Q[3].re = V[15].re / y;
        Q[3].im = 0.0;
      } else if (V[15].re == 0.0) {
        Q[3].re = 0.0;
        Q[3].im = V[15].im / y;
      } else {
        Q[3].re = V[15].re / y;
        Q[3].im = V[15].im / y;
      }
    }
    break;

   default:
    {
      double a;
      double b;
      double c;
      double old_lambda;

      //  method name is 'newtown'
      c = coder::det(WW);
      for (i = 0; i < 3; i++) {
        b_tmp[3 * i] = MM[i];
        b_tmp[3 * i + 1] = MM[i + 3];
        b_tmp[3 * i + 2] = MM[i + 6];
      }

      b = -8.0 * coder::b_det(b_tmp);
      for (i = 0; i < 3; i++) {
        d = MM[i];
        ar = MM[i + 3];
        y = MM[i + 6];
        for (iter = 0; iter < 3; iter++) {
          b_y[i + 3 * iter] = (d * b_tmp[3 * iter] + ar * b_tmp[3 * iter + 1]) +
            y * b_tmp[3 * iter + 2];
        }
      }

      a = -2.0 * ((b_y[0] + b_y[4]) + b_y[8]);
      lambda = 1.0;
      old_lambda = 1.0;
      iter = 0;
      while ((std::abs(old_lambda - lambda) > 1.0E-8) && (iter <= 50)) {
        old_lambda = lambda;
        t = lambda * lambda + a / 2.0;
        lambda -= (((t * t + b * lambda) + c) - a * a / 4.0) / (4.0 * lambda * t
          + b);
        iter++;
      }

      for (i = 0; i < 16; i++) {
        WW[i] -= lambda * static_cast<double>(b_b[i]);
      }

      d = WW[4] / WW[0];
      ar = d;
      WW[4] = d;
      d = WW[5] - WW[1] * d;
      WW[5] = d;
      WW[6] -= WW[2] * ar;
      d = WW[8] / WW[0];
      ar = d;
      WW[8] = d;
      d = WW[9] - WW[1] * d;
      WW[9] = d;
      WW[10] -= WW[2] * ar;
      d = WW[12] / WW[0];
      ar = d;
      WW[12] = d;
      d = WW[13] - WW[1] * d;
      WW[14] -= WW[2] * ar;
      ar = WW[9] / WW[5];
      y = ar;
      WW[9] = ar;
      ar = WW[8] - WW[4] * ar;
      WW[8] = ar;
      WW[10] -= WW[6] * y;
      ar = d / WW[5];
      y = ar;
      WW[13] = ar;
      ar = WW[12] - WW[4] * ar;
      WW[14] -= WW[6] * y;
      d = WW[14] / WW[10];
      y = d;
      WW[14] = d;
      d = ar - WW[8] * d;
      WW[13] -= WW[9] * y;

      //  return the result Q
      lambda = 3.3121686421112381E-170;
      old_lambda = std::abs(d);
      if (old_lambda > 3.3121686421112381E-170) {
        y = 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / 3.3121686421112381E-170;
        y = t * t;
      }

      old_lambda = std::abs(WW[13]);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      old_lambda = std::abs(WW[14]);
      if (old_lambda > lambda) {
        t = lambda / old_lambda;
        y = y * t * t + 1.0;
        lambda = old_lambda;
      } else {
        t = old_lambda / lambda;
        y += t * t;
      }

      if (1.0 > lambda) {
        y = y * lambda * lambda + 1.0;
        lambda = 1.0;
      } else {
        t = 1.0 / lambda;
        y += t * t;
      }

      y = lambda * std::sqrt(y);
      Q[0].re = d / y;
      Q[0].im = 0.0;
      Q[1].re = WW[13] / y;
      Q[1].im = 0.0;
      Q[2].re = WW[14] / y;
      Q[2].im = 0.0;
      Q[3].re = -1.0 / y;
      Q[3].im = 0.0;
    }
    break;
  }
}

//
// File trailer for FLAE.cpp
//
// [EOF]
//
